From 58437010f9cde5bbb29d582a62e875eaf5ec555c Mon Sep 17 00:00:00 2001 From: robertl Date: Tue, 14 Oct 2008 16:00:00 +0000 Subject: [PATCH] Rodney Lorrimar improves Navilink. Serializes nuking and transferring. Improves errors. Improves BGT-31 support. --- defs.h | 8 +++ hsa_ndv.c | 3 - igo8.c | 2 - navilink.c | 173 +++++++++++++++++++++++++++++++++++------------------ nmea.c | 1 - 5 files changed, 123 insertions(+), 64 deletions(-) diff --git a/defs.h b/defs.h index a857c290f..e96eba6f8 100644 --- a/defs.h +++ b/defs.h @@ -51,6 +51,14 @@ # define M_PI 3.14159265358979323846 #endif +#ifndef FALSE +# define FALSE 0 +#endif + +#ifndef TRUE +# define TRUE !FALSE +#endif + #define FEET_TO_METERS(feetsies) ((feetsies) * 0.3048) #define METERS_TO_FEET(meetsies) ((meetsies) * 3.2808399) diff --git a/hsa_ndv.c b/hsa_ndv.c index 1ceb8fd36..867062280 100644 --- a/hsa_ndv.c +++ b/hsa_ndv.c @@ -50,9 +50,6 @@ arglist_t hsa_ndv_args[] = { #define MYNAME "HsaNdv" #define MY_CBUF 4096 -#define TRUE 1 -#define FALSE 0 - #if ! HAVE_LIBEXPAT static void diff --git a/igo8.c b/igo8.c index 3f8f72b4b..9698f2d53 100644 --- a/igo8.c +++ b/igo8.c @@ -67,8 +67,6 @@ #include "cet.h" #include "cet_util.h" -#define TRUE 1 -#define FALSE 0 #define FLOAT_TO_INT(x) ((int)((x) + ((x)<0?-0.5:0.5))) #define IGO8_HEADER_SIZE (sizeof(igo8_id_block) + 256) #define MYNAME "IGO8" diff --git a/navilink.c b/navilink.c index a35ae8d1f..e23c5d21c 100644 --- a/navilink.c +++ b/navilink.c @@ -43,6 +43,11 @@ static waypoint **route_waypts; static unsigned *route_ids; static unsigned route_id_ptr; +static enum { + READING, + WRITING +} operation = READING; + #define SERIAL_TIMEOUT 8000 #define MAX_WAYPOINTS 1000 @@ -246,15 +251,28 @@ read_word(void) return (buffer[1] << 8) | buffer[0]; } -static void -read_packet(unsigned type, void *payload, unsigned minlength, unsigned maxlength) +/* + * Read a protocol packet into payload. + * + * handle_nak determines behaviour when a PID_NAK packet is read from + * the device: + * - if handle_nak is FALSE, a fatal error will be raised. + * - if handle_nak is TRUE, read_packet will simply return FALSE. + * + * Returns TRUE if the packet was successfully read into payload. + */ +static int +read_packet(unsigned type, void *payload, + unsigned minlength, unsigned maxlength, + int handle_nak) { unsigned size; unsigned char *data; unsigned checksum; if (read_word() != 0xa2a0) { - fatal(MYNAME ": Protocol error: Bad packet header\n"); + fatal(MYNAME ": Protocol error: Bad packet header." + " Is your NaviGPS in NAVILINK mode?\n"); } if ((size = read_word()) <= minlength) { @@ -272,6 +290,10 @@ read_packet(unsigned type, void *payload, unsigned minlength, unsigned maxlength #endif if (data[0] != type) { + if (handle_nak && data[0] == PID_NAK) { + return FALSE; + } + fatal(MYNAME ": Protocol error: Bad packet type (expected 0x%02x but got 0x%02x)\n", type, data[0]); } @@ -291,7 +313,7 @@ read_packet(unsigned type, void *payload, unsigned minlength, unsigned maxlength xfree(data); - return; + return TRUE; } static time_t @@ -432,7 +454,9 @@ serial_read_waypoints(void) } write_packet(PID_QRY_INFORMATION, NULL, 0); - read_packet(PID_DATA, information, sizeof(information), sizeof(information)); + read_packet(PID_DATA, information, + sizeof(information), sizeof(information), + FALSE); total = le_read16(information + 0); @@ -452,7 +476,7 @@ serial_read_waypoints(void) waypoints = xmalloc(count * 32); - read_packet(PID_DATA, waypoints, count * 32, count * 32); + read_packet(PID_DATA, waypoints, count * 32, count * 32, FALSE); for (w = waypoints; w < waypoints + count * 32; w = w + 32) { if (global_opts.masked_objective & WPTDATAMASK) { @@ -473,15 +497,25 @@ serial_read_waypoints(void) return waypts; } -static void -serial_write_waypoint(const waypoint *waypt) +static unsigned int +serial_write_waypoint_packet(const waypoint *waypt) { unsigned char data[32]; unsigned char id[2]; encode_waypoint(waypt, data); write_packet(PID_ADD_A_WAYPOINT, data, sizeof(data)); - read_packet(PID_DATA, id, sizeof(id), sizeof(id)); + if (!read_packet(PID_DATA, id, sizeof(id), sizeof(id), TRUE)) { + fatal(MYNAME ": Could not write waypoint.\n"); + } + + return le_read16(id); +} + +static void +serial_write_waypoint(const waypoint *waypt) +{ + serial_write_waypoint_packet(waypt); } static void @@ -493,7 +527,9 @@ serial_read_track(void) route_head *track; write_packet(PID_QRY_INFORMATION, NULL, 0); - read_packet(PID_DATA, information, sizeof(information), sizeof(information)); + read_packet(PID_DATA, information, + sizeof(information), sizeof(information), + FALSE); address = le_read32(information + 4); total = le_read16(information + 12); @@ -515,7 +551,7 @@ serial_read_track(void) trackpoints = xmalloc(count * 32); - read_packet(PID_DATA, trackpoints, count * 32, count * 32); + read_packet(PID_DATA, trackpoints, count * 32, count * 32, FALSE); write_packet(PID_ACK, NULL, 0); for (t = trackpoints; t < trackpoints + count * 32; t = t + 32) { @@ -538,7 +574,9 @@ serial_write_track(void) unsigned char data[7]; write_packet(PID_QRY_INFORMATION, NULL, 0); - read_packet(PID_DATA, information, sizeof(information), sizeof(information)); + read_packet(PID_DATA, information, + sizeof(information), sizeof(information), + FALSE); address = le_read32(information + 4); total = le_read16(information + 12); @@ -550,7 +588,7 @@ serial_write_track(void) write_packet(PID_WRITE_TRACKPOINTS, data, sizeof(data)); gb_sleep(10000); write_packet(PID_DATA, track_data, track_data_ptr - track_data); - read_packet(PID_CMD_OK, NULL, 0, 0); + read_packet(PID_CMD_OK, NULL, 0, 0, FALSE); track_data_ptr = track_data; } @@ -593,7 +631,9 @@ serial_read_routes(waypoint **waypts) unsigned char r; write_packet(PID_QRY_INFORMATION, NULL, 0); - read_packet(PID_DATA, information, sizeof(information), sizeof(information)); + read_packet(PID_DATA, information, + sizeof(information), sizeof(information), + FALSE); routec = information[2]; @@ -608,7 +648,7 @@ serial_read_routes(waypoint **waypts) payload[6] = 0x01; write_packet(PID_QRY_ROUTE, payload, sizeof(payload)); - read_packet(PID_DATA, routedata, 64, sizeof(routedata)); + read_packet(PID_DATA, routedata, 64, sizeof(routedata), FALSE); route = route_head_alloc(); route->rte_num = routedata[2]; @@ -655,15 +695,7 @@ serial_write_route_point(const waypoint *waypt) } if (w == MAX_WAYPOINTS) { - unsigned char data[32]; - unsigned char id[2]; - - encode_waypoint(waypt, data); - write_packet(PID_ADD_A_WAYPOINT, data, sizeof(data)); - read_packet(PID_DATA, id, sizeof(id), sizeof(id)); - - w = le_read16(id); - + w = serial_write_waypoint_packet(waypt); route_waypts[w] = waypt_dupe(waypt); } @@ -717,7 +749,9 @@ serial_write_route_end(const route_head *route) } write_packet(PID_ADD_A_ROUTE, data, 32 + src * 32); - read_packet(PID_DATA, id, sizeof(id), sizeof(id)); + if (!read_packet(PID_DATA, id, sizeof(id), sizeof(id), TRUE)) { + fatal(MYNAME ": Could not add route.\n"); + } xfree(data); xfree(route_ids); @@ -802,51 +836,67 @@ file_write_route_end(const route_head *track) } static void -navilink_common_init(const char *name, const char *mode) +nuke(void) { - if (gbser_is_serial(name)) { - if ((serial_handle = gbser_init(name)) == NULL) { - fatal(MYNAME ": Could not open serial port %s\n", name); - } - - if (gbser_set_port(serial_handle, 115200, 8, 0, 1) != gbser_OK) { - fatal(MYNAME ": Can't configure port\n"); - } + if (nuketrk) { + unsigned char information[32]; + unsigned char data[7]; - write_packet(PID_SYNC, NULL, 0); - read_packet(PID_ACK, NULL, 0, 0); + write_packet(PID_QRY_INFORMATION, NULL, 0); + read_packet(PID_DATA, information, + sizeof(information), sizeof(information), + FALSE); - if (nuketrk) { - unsigned char information[32]; - unsigned char data[7]; + le_write32(data + 0, le_read32(information + 4)); + le_write16(data + 4, 0); + data[6] = 0; - write_packet(PID_QRY_INFORMATION, NULL, 0); - read_packet(PID_DATA, information, sizeof(information), sizeof(information)); + write_packet(PID_ERASE_TRACK, data, sizeof(data)); + read_packet(PID_CMD_OK, NULL, 0, 0, FALSE); + } - le_write32(data + 0, le_read32(information + 4)); - le_write16(data + 4, 0); - data[6] = 0; + if (nukerte) { + unsigned char data[4]; - write_packet(PID_ERASE_TRACK, data, sizeof(data)); - read_packet(PID_CMD_OK, NULL, 0, 0); + le_write32(data, 0x00f00000); + write_packet(PID_DEL_ALL_ROUTE, data, sizeof(data)); + if (!read_packet(PID_ACK, NULL, 0, 0, TRUE)) { + fatal(MYNAME ": Could not nuke all routes.\n"); } + } - if (nukerte) { - unsigned char data[4]; + if (nukewpt) { + unsigned char data[4]; - le_write32(data, 0x00f00000); - write_packet(PID_DEL_ALL_ROUTE, data, sizeof(data)); - read_packet(PID_CMD_OK, NULL, 0, 0); + le_write32(data, 0x00f00000); + write_packet(PID_DEL_ALL_WAYPOINT, data, sizeof(data)); + if (!read_packet(PID_ACK, NULL, 0, 0, TRUE)) { + fatal(MYNAME ": You have to nuke all routes before nuking waypoints.\n"); + /* perhaps a better action would be to nuke routes for user. + * i.e. set nukerte when nukewpt is set */ } + } +} - if (nukewpt) { - unsigned char data[4]; +static void +navilink_common_init(const char *name) +{ + if (gbser_is_serial(name)) { + if ((serial_handle = gbser_init(name)) == NULL) { + fatal(MYNAME ": Could not open serial port %s\n", name); + } - le_write32(data, 0x00f00000); - write_packet(PID_DEL_ALL_WAYPOINT, data, sizeof(data)); - read_packet(PID_ACK, NULL, 0, 0); + if (gbser_set_port(serial_handle, 115200, 8, 0, 1) != gbser_OK) { + fatal(MYNAME ": Can't configure port\n"); } + write_packet(PID_SYNC, NULL, 0); + read_packet(PID_ACK, NULL, 0, 0, FALSE); + + /* nuke data before writing */ + if (operation == WRITING) + nuke(); + write_waypoint = serial_write_waypoint; write_track_start = serial_write_track_start; write_track_point = serial_write_track_point; @@ -855,6 +905,7 @@ navilink_common_init(const char *name, const char *mode) write_route_point = serial_write_route_point; write_route_end = serial_write_route_end; } else { + char *mode = operation == READING ? "r" : "w+"; file_handle = gbfopen(name, mode, MYNAME); write_waypoint = file_write_waypoint; @@ -872,19 +923,25 @@ navilink_common_init(const char *name, const char *mode) static void navilink_rd_init(const char *name) { - navilink_common_init(name, "r"); + operation = READING; + navilink_common_init(name); } static void navilink_wr_init(const char *name) { - navilink_common_init(name, "w+"); + operation = WRITING; + navilink_common_init(name); } static void navilink_deinit(void) { if (serial_handle) { + /* nuke data after reading */ + if (operation == READING) + nuke(); + if (poweroff) { write_packet(PID_QUIT, NULL, 0); } diff --git a/nmea.c b/nmea.c index de35d97d7..abd81700e 100644 --- a/nmea.c +++ b/nmea.c @@ -254,7 +254,6 @@ nmea_rd_init(const char *fname) last_waypt = NULL; last_time = -1; datum = DATUM_WGS84; - static int had_checksum = 0; CHECK_BOOL(opt_gprmc); CHECK_BOOL(opt_gpgga); -- 2.30.2